#include "drive_to_path.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <geometry_msgs/msg/twist.hpp>
#include <chrono>
#include <ros/package.h>

// #define PACKAGE_NAME "agv_navmap"
constexpr const char* MAP_FILE_NAME = "/map/mapTest.xml";
constexpr float STOP_DIS = 0.03f;
constexpr float SLOW_DIS = 0.15f;
constexpr float ANG_ADJUST_DIS = 0.35f;
constexpr float STOP_ANG_IN = 2.50f;
constexpr float STOP_ANG_OUT = 5.00f;
constexpr float SLOW_ANG = 15.00f;
// constexpr float PI = 3.14159265f;

/**
 * @brief DriveToPath类的构造函数
 *
 * 初始化DriveToPath对象，并设置rclcpp::Node参数。
 *
 * @param nh rclcpp::Node对象，用于与ROS系统通信
 */
DriveToPath::DriveToPath(rclcpp::Node& nh) 
    : dynamic_reconfigure_server_(nullptr),
    nh_(nh) 
{
  Init();
}

/**
 * @brief DriveToPath析构函数
 *
 * 析构函数用于在对象销毁时释放资源。
 * 如果存在动态配置服务器，则删除该服务器，并将其指针设置为nullptr。
 */
DriveToPath::~DriveToPath() 
{
    if (dynamic_reconfigure_server_) {
        delete dynamic_reconfigure_server_;
        dynamic_reconfigure_server_ = nullptr;
    }
}

/**
 * @brief 初始化DriveToPath类
 *
 * 初始化DriveToPath类，包括设置控制模式、初始化外部参数、初始化变量、订阅和发布ROS话题等。
 *
 * @return 如果初始化成功，则返回true；如果已初始化，则返回false。
 */
bool DriveToPath::Init() 
{
    if (drive_to_path_init_ok_) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvNavigate"), "[DriveToPath] DriveToPath is already initialized");
        return false;
    }
    cur_ctrl_mode_ = static_cast<int>(CtrlMode::NoneMode);
    last_ctrl_mode_ = static_cast<int>(CtrlMode::NoneMode);
    ExternalParamsInit();
    VariableInit();
    vel_pub_ = nh_.advertise<geometry_msgs::msg::Twist>("base/twist_mux/keyboard_vel", 1);
    agvmode_sub_ = nh_.subscribe("agvmode", 1, &DriveToPath::AgvModeCallback, this);
    allow_move_pub_ = nh_.advertise<std_msgs::msg::Bool>("halfauto_allow_move", 1);
    allow_move_sub_ = nh_.subscribe("halfauto_allow_move", 1, &DriveToPath::AllowMoveCallback, this);
    dynamic_reconfigure_server_ = new dynamic_reconfigure::Server<agv_navigate::paramsConfig>(nh_);
    dynamic_reconfigure_callback_ = boost::bind(&DriveToPath::ReconfigCB, this, _1, _2);
    dynamic_reconfigure_server_->setCallback(dynamic_reconfigure_callback_);
    drive_to_path_init_ok_ = true;
    return true;
}

/**
 * @brief 初始化外部参数
 *
 * 从ROS参数服务器读取外部参数并初始化。
 *
 * @note 此函数使用ROS命名空间中的节点名称来读取参数。
 */
void DriveToPath::ExternalParamsInit() 
{
    std::string str_node_name = ros::this_node::getName();
    nh_.param(str_node_name + "/mode", mode_, 1);
    nh_.param(str_node_name + "/dis_max", dis_max_, 10.0);
    nh_.param(str_node_name + "/high_speed", high_speed_, 0.3);
    nh_.param(str_node_name + "/low_speed", low_speed_, 0.1);
}

/**
 * @brief 初始化变量
 *
 * 此函数用于初始化DriveToPath类的各个成员变量，包括路径状态、位置、目标位置、角度等。
 */
void DriveToPath::VariableInit() 
{
    is_on_path_ = false;
    is_allow_move_ = false;
    cur_state_ = static_cast<int>(RunState::NoneState);
    cur_pos_x_ = 0;
    cur_pos_y_ = 0;
    cur_pos_t_ = 0;
    cur_pos_is_update_ = false;
    cur_target_is_update_ = false;
    cur_state_is_update_ = false;
    cur_target_x_ = 0;
    cur_target_y_ = 0;
    cur_target_t_ = 0;
    end_target_t_ = 0;
    cur_dis_ = 0;
    cur_ang_ = 0;
    end_ang_ = 0;
    stop_ang_out_ = STOP_ANG_OUT;
    sign_ = 1;
}

/**
 * @brief 重新配置回调函数
 *
 * 当接收到重新配置请求时，此函数将被调用。
 *
 * @param config 配置参数对象
 * @param unused_level 未使用的参数
 */
void DriveToPath::ReconfigCB(agv_navigate::paramsConfig &config, uint32_t unused_level)
{
    RCLCPP_INFO(rclcpp::get_logger("AgvNavigate"), "[DriveToPath] Reconfigure request: %d, %.2f, %.2f, %.2f",
        config.mode, config.dis_max, config.high_speed, config.low_speed);
    mode_ = config.mode;
    dis_max_ = config.dis_max;
    high_speed_ = config.high_speed;
    low_speed_ = config.low_speed;
}

//获取当前控制模式，当从其他模式切至半自动模式时，开启两条线程执行功能Merge the three packages agv_navigate, agv_world, and agv_drive_path into one package agv_navigate
// 2023.12.18 modify by haibo
/**
 * @brief 回调函数，用于处理AGV模式切换
 *
 * 当AGV模式发生变化时，该函数会被调用。函数内部会根据新的AGV模式更新当前的控制模式，
 * 并根据需要在特定情况下启动新的线程以获取当前位置或驱动AGV。
 *
 * @param msg 指向agv_msgs::msg::AGVMode消息的指针，包含新的AGV模式信息
 */
void DriveToPath::AgvModeCallback(const agv_msgs::msg::AGVMode::ConstSharedPtr& msg) 
{
    std::lock_guard<std::mutex> lock(mutex_);
    uint8_t agv_mode = msg->mode;
    bool manual_mode = agv_mode == agv_msgs::msg::AGVMode::MANAUL;   
    bool auto_mode = agv_mode == agv_msgs::msg::AGVMode::AUTOMATIC;
    bool semi_mode = agv_msgs::msg::AGVMode::SEMI_AUTOMATIC;
    last_ctrl_mode_ = cur_ctrl_mode_;
    if (manual_mode)
        cur_ctrl_mode_ = static_cast<int>(CtrlMode::ManualMode);
    else if (semi_mode)
        cur_ctrl_mode_ = static_cast<int>(CtrlMode::HalfAutoMode);
    else if (auto_mode)
        cur_ctrl_mode_ = static_cast<int>(CtrlMode::AutoMode);

    if (last_ctrl_mode_ != cur_ctrl_mode_ && cur_ctrl_mode_ == static_cast<int>(CtrlMode::HalfAutoMode)) {
        if (mode_ == static_cast<int>(RunMode::SafeRun) || mode_ == static_cast<int>(RunMode::AutoRun)) {
            ExternalParamsInit();
            VariableInit();
            get_cur_pos_thread_ = std::thread(GetCurPosThreadFun, this);
            get_cur_pos_thread_.detach();
            agv_driving_thread_ = std::thread(AgvDrivingThreadFun, this);
            agv_driving_thread_.detach();
        }
    }
}

/**
 * @brief 获取指定位置的位值
 *
 * 根据指定的值和索引，返回该索引位置上的位值。
 *
 * @param value 待检查的整数值
 * @param index 索引位置，从0开始计数
 *
 * @return 如果指定索引位置上的位值为1，则返回true；否则返回false
 */
bool DriveToPath::GetBit(int value, int index) 
{
    return (value >> index & 1) == 1;
}

/**
 * @brief 允许移动回调函数
 *
 * 当收到允许移动的消息时，调用此回调函数更新允许移动的状态。
 *
 * @param msg 允许移动的消息指针
 */
void DriveToPath::AllowMoveCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) 
{
    std::lock_guard<std::mutex> lock(mutex_);
    is_allow_move_ = msg->data;
}

/**
 * @brief 获取当前位置的线程函数
 *
 * 该函数用于在一个独立的线程中不断更新当前位置信息。
 *
 * @param p 指向driveToPath对象的指针
 */
void DriveToPath::GetCurPosThreadFun(DriveToPath *p)
{
    geometry_msgs::msg::TransformStamped agvtf;
    tf2_ros::TransformListener listener;
    tf2::StampedTransform basetomap;
    while (rclcpp::ok() && (p->cur_ctrl_mode_ == static_cast<int>(CtrlMode::HalfAutoMode)) && (p->cur_dis_ < p->dis_max_)) {
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
        // #1 计算当前位姿
        try {
            // Get base link global coordinates
            bool ok = listener.waitForTransform("map", "base_link", rclcpp::Time(0), rclcpp::Duration(0.02));
            (void)(ok);
            listener.lookupTransform("map", "base_link", rclcpp::Time(0), basetomap);
            agvtf.transform.rotation.w = basetomap.getRotation().getW();
            agvtf.transform.rotation.x = basetomap.getRotation().getX();
            agvtf.transform.rotation.y = basetomap.getRotation().getY();
            agvtf.transform.rotation.z = basetomap.getRotation().getZ();
            std::lock_guard<std::mutex> lock(p->mutex_);
            p->cur_pos_x_ = basetomap.getOrigin().getX();
            p->cur_pos_y_ = basetomap.getOrigin().getY();
            double pos = tf2::getYaw(agvtf.transform.rotation) * (180 / PI);
            p->cur_pos_t_  = pos > 0 ? pos : (pos + 360);
            p->cur_pos_is_update_ = true;
        } catch (tf::TransformException &ex) {
            // p->m_mutex.unlock();
            continue;
        };
        // #2 目标位姿更新后，计算与目标点的相对位姿
        if (p->cur_target_is_update_) {
            double dx = p->cur_target_x_ - p->cur_pos_x_;
            double dy = p->cur_target_y_ - p->cur_pos_y_;
            // DEBUG_OUT("m_curTargetX=" << p->m_curTargetX << ",m_curTargetY=" << p->m_curTargetY << ",m_curPosX=" << p->m_curPosX << ",m_curPosY=" << p->m_curPosY);
            if ((dx == 0) && (dy == 0)) {
                p->cur_target_t_ = p->cur_pos_t_;
            } else {
                double theta = std::atan2(dy, dx) * (180 / PI);
                // degree   -180+180 ---> 0-360
                p->cur_target_t_ = theta > 0 ? theta : (theta + 360);
            }

            //#2.1 计算距离、角度偏差
            p->cur_dis_ = std::sqrt(dx * dx + dy * dy);
            //角度大阈值：35cm以上-为5，35cm以内-arcsin(距离阈值/当前距离)
            p->stop_ang_out_ = p->cur_dis_ < ANG_ADJUST_DIS ? std::asin(STOP_DIS / p->cur_dis_) * (180 / PI) : STOP_ANG_OUT;

            //#2.2 计算当前角度与目标角度偏差（顺时针、逆时针取小值）
            double ang1 = p->cur_target_t_ - p->cur_pos_t_;
            double ang2;
            if (p->cur_target_t_ >= p->cur_pos_t_)
                ang2 = p->cur_target_t_ - (p->cur_pos_t_ + 360);
            else
                ang2 = (p->cur_target_t_ + 360) - p->cur_pos_t_;
            p->cur_ang_ = std::fabs(ang1) < std::fabs(ang2) ? ang1 : ang2;

            //#2.3 计算当前角度与最终角度偏差（顺时针、逆时针取小值）
            double ang3 = p->end_target_t_ - p->cur_pos_t_;
            double ang4;
            if (p->end_target_t_ >= p->cur_pos_t_)
                ang4 = p->end_target_t_ - (p->cur_pos_t_ + 360);
            else
                ang4 = (p->end_target_t_ + 360) - p->cur_pos_t_;
            p->end_ang_ = std::fabs(ang3) < std::fabs(ang4) ? ang3 : ang4;

            //#2.4 根据两偏差，判断当前状态//0-角度√距离√；1-角度×距离×；2-角度√距离×；3-角度×距离√；
            if (p->cur_dis_ < STOP_DIS && std::fabs(p->end_ang_) < STOP_ANG_IN) {
                p->is_on_path_ = true;
                p->cur_state_ = static_cast<int>(RunState::NoneState);
            } else {
                p->is_on_path_  = false;
                //已经达到最终角调整状态后，将不再切换1、2状态，旋转引起的位置偏差被忽略
                if ((p->cur_dis_ >= STOP_DIS) && (p->cur_state_ != static_cast<int>(RunState::FinalAngleAdjust))) {
                    if (std::fabs(p->cur_ang_) >= p->stop_ang_out_)
                        p->cur_state_ = static_cast<int>(RunState::StartAngleAdjust);
                    else if (std::fabs(p->cur_ang_) < STOP_ANG_IN)
                        p->cur_state_ = static_cast<int>(RunState::MoveToTarget);
                    else {
                        if (p->cur_state_ == static_cast<int>(RunState::NoneState))
                        p->cur_state_ = static_cast<int>(RunState::StartAngleAdjust);
                    }
                } else {
                if (std::fabs(p->end_ang_) >= STOP_ANG_IN)
                    p->cur_state_ = static_cast<int>(RunState::FinalAngleAdjust);
                }
            }
            p->cur_state_is_update_ = true;
        }
    }
    // pThis->m_curPosIsUpdate = false;
    // pThis->m_curStateIsUpdate = false;
    // pThis->m_curState = 0;
    return;
}

/**
 * @brief 执行行驶操作
 *
 * 根据当前状态，调整速度和角度，控制小车行驶到指定路径。
 */
void DriveToPath::DoDriving()
{
    while (rclcpp::ok() && !is_on_path_ && (cur_ctrl_mode_ == static_cast<int>(CtrlMode::HalfAutoMode)) && (cur_dis_ < dis_max_)) {
        geometry_msgs::msg::Twist cmd;
        double vline = 0;
        double vang = 0;
        std::lock_guard<std::mutex> lock(mutex_);
        //角度调整时，角度达到小阈值则认为状态1完成，运动过程中，角度超出大阈值时才认为不满足要求，将重新切回状态1
        if (cur_state_ == static_cast<int>(RunState::StartAngleAdjust)) {
            sign_ = cur_ang_ >= 0 ? 1 : -1;
            if (std::fabs(cur_ang_) > SLOW_ANG) {
                vline = 0;
                vang = high_speed_ * sign_;
            } else if (std::fabs(cur_ang_) < STOP_ANG_IN) {
                vline = 0;
                vang = 0;
            } else {
                vline = 0;
                vang = low_speed_ * sign_;
            }
        } else if (cur_state_ == static_cast<int>(RunState::MoveToTarget)) {
            sign_ = cur_ang_ >= 0 ? 1 : -1;
            if (cur_dis_ > SLOW_DIS) {
                vline = high_speed_;
                if (std::fabs(cur_ang_) < STOP_ANG_IN)
                    vang = 0;
                else
                    vang = high_speed_ * sign_ * (std::fabs(cur_ang_) - STOP_ANG_IN) / (stop_ang_out_ - STOP_ANG_IN) * (stop_ang_out_ / STOP_ANG_IN); 
            } else if (cur_dis_ < STOP_DIS) {
                vline = 0;
                vang = 0;
            } else {
                vline = low_speed_;
                if (std::fabs(cur_ang_) < STOP_ANG_IN)
                    vang = 0;
                else
                    vang = low_speed_ * sign_ * (std::fabs(cur_ang_) - STOP_ANG_IN) / (stop_ang_out_ - STOP_ANG_IN) * (stop_ang_out_ / STOP_ANG_OUT); 
                if (vang > high_speed_)
                    vang = high_speed_;
            }
        } else if (cur_state_ == static_cast<int>(RunState::FinalAngleAdjust)) {
            sign_ = end_ang_ >= 0 ? 1 : -1;
            if (std::fabs(end_ang_) > SLOW_ANG) {
                vline = 0;
                vang = high_speed_ * sign_;
            } else if (std::fabs(end_ang_) < STOP_ANG_IN) {
                vline = 0;
                vang = 0;
            } else {
                vline = 0;
                vang = low_speed_ * sign_;
            }
        }

        if ((is_allow_move_) || (mode_ == static_cast<int>(RunMode::AutoRun))) {
            cmd.linear.x = vline;
            cmd.angular.z = vang;
            vel_pub_.publish(cmd);
            is_allow_move_ = false;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
}

//矢量算法求点到线的最近点及最短距离
/**
 * @brief 将一个点投影到一条路径上，并计算投影点的位置和方向
 *
 * 将给定的点(x, y)投影到路径Path上，并返回投影点的位置(TargetPos结构体)和路径的朝向。
 *
 * @param x 给定点的x坐标
 * @param y 给定点的y坐标
 * @param Path 指向路径对象的指针
 *
 * @return 投影点的位置和方向(TargetPos结构体)
 */
TargetPos DriveToPath::PointToLine(double x, double y, Path* path)
{
    // m_mutex.lock();
    TargetPos pos;
    LinePath* line_path = dynamic_cast<LinePath*>(path);
    if (line_path) {
        double x1 = line_path->GetStartPoint().x_;
        double y1 = line_path->GetStartPoint().y_;
        double x2 = line_path->GetEndPoint().x_;
        double y2 = line_path->GetEndPoint().y_;
        double cross = (x2 - x1) * (x - x1) + (y2 - y1) * (y - y1);
        double d2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
        if (cross <= 0) {
            pos.dis = std::sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));
            pos.x = x1;
            pos.y = y1;
        } else if (cross >= d2) {
            pos.dis = std::sqrt((x - x2) * (x - x2) + (y - y2) * (y - y2));
            pos.x = x2;
            pos.y = y2;
        } else {
            double r = cross / d2;
            double px = x1 + (x2 - x1) * r;
            double py = y1 + (y2 - y1) * r;
            pos.dis = std::sqrt((x - px) * (x - px) + (y - py) * (y - py));
            pos.x = px;
            pos.y = py;
        }
        pos.theta = line_path->GetHeading(line_path->GetStartNode()).radian_ * (180 / PI);
    }
  return pos;
}

/**
 * @brief 将给定坐标点转换为贝塞尔路径上的最近点
 *
 * 根据给定的坐标点 (x, y) 和贝塞尔路径 path，找到贝塞尔路径上距离该坐标点最近的点，
 * 并返回该点的坐标、距离以及方向角。
 *
 * @param x 给定坐标点的 x 坐标
 * @param y 给定坐标点的 y 坐标
 * @param path 贝塞尔路径对象指针
 *
 * @return 返回最近点的位置信息，包括距离 (dis)、x 坐标 (x)、y 坐标 (y) 和方向角 (theta)
 */
TargetPos DriveToPath::PointToBezier(double x, double y, Path *path)
{
    std::lock_guard<std::mutex> lock(mutex_);
    TargetPos pos;
    BezierPath* bezier_path = dynamic_cast<BezierPath*>(path);
    if (bezier_path) {
        double x1 = bezier_path->GetStartPoint().x_;
        double y1 = bezier_path->GetStartPoint().y_;
        double x2 = bezier_path->GetEndPoint().x_;
        double y2 = bezier_path->GetEndPoint().y_;
        double d1 = std::sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));
        double d2 = std::sqrt((x - x2) * (x - x2) + (y - y2) * (y - y2));
        if (d1 <= d2) {
            pos.dis = d1;
            pos.x = x1;
            pos.y = y1;
            pos.theta = bezier_path->GetHeading(bezier_path->GetStartNode()).radian_ * (180 / PI);
        } else {
            pos.dis = d2;
            pos.x = x2;
            pos.y = y2;
            pos.theta = bezier_path->GetHeading(bezier_path->GetEndNode()).radian_ * (180 / PI);
        }
    }
    return pos;
}

/**
 * @brief 将指定点转换到路径的一侧，并返回该点的目标位置
 *
 * 将指定的点 (x, y) 转换到给定路径 (path) 的一侧，并返回转换后的目标位置。
 *
 * @param x 指定点的横坐标
 * @param y 指定点的纵坐标
 * @param path 给定的路径对象
 *
 * @return 转换后的目标位置
 */
TargetPos DriveToPath::PointToSide(double x, double y, Path* path) {
    std::lock_guard<std::mutex> lock(mutex_);
    TargetPos pos;
    // TODO: 实现具体逻辑
    return pos;
}

/**
 * @brief 获取最终的目标点
 *
 * 从当前地图中获取所有路径，并计算得到离当前位置最近的目标点。
 *
 * @return 返回最终的目标点
 */
TargetPos DriveToPath::GetFinallyTargetPoint()
{
    //#1  加载地图，获取所有路径及其端点（路径组）
    World cur_world;
    TargetPos target_pos;
    std::string map_file = std::string(getenv("HOME")) + MAP_FILE_NAME;
    bool load = cur_world.LoadMap(map_file);
    if (!load) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvNavigate"), "[DriveToPath] HalfAutoMode LoadMap failed");
        return target_pos;
    }
    bool set_first_flag = true;
    PathBase& path_base = *cur_world.GetPathBaseObject();
    int path_count = path_base.path_count_;
    for (int i = 0; i < path_count; i++) {
        TargetPos pos;
        Path* path = path_base.path_index_[i].ptr_;
        if (path->type_ == static_cast<USHORT>(PathType::LINE_TYPE))
            pos = PointToLine(cur_pos_x_, cur_pos_y_, path);
        else if (path->type_ == static_cast<USHORT>(PathType::BEZIER_TYPE))
            pos = PointToBezier(cur_pos_x_, cur_pos_y_, path);
        else if (path->type_ == static_cast<USHORT>(PathType::SIDE_TYPE))
            pos = PointToSide(cur_pos_x_, cur_pos_y_, path);
        if (pos.dis == 0 && pos.x == 0 && pos.y == 0 && pos.theta == 0)
            continue;
        if (set_first_flag) {
            target_pos = pos;
            set_first_flag = false;
        } else {
            if (pos.dis < target_pos.dis)
                target_pos = pos;
        }
    }
    return target_pos;
}

// void driveToPath::initTarget()
// {
//     m_curTargetX = 0;
//     m_curTargetY = 0;
//     m_endTargetT = 0;
//     m_curTargetT = 0;
// }

/**
 * @brief 更新目标位置
 *
 * 使用给定的目标位置更新当前目标位置。
 *
 * @param pos 目标位置结构体，包含目标位置的 x、y 坐标和角度 theta。
 */
void DriveToPath::UpdateTarget(TargetPos pos) 
{
    std::lock_guard<std::mutex> lock(mutex_);
    cur_target_x_ = pos.x;
    cur_target_y_ = pos.y;
    end_target_t_ = pos.theta;
}

//行驶至路线-线程函数
/**
 * @brief AgvDrivingThreadFun函数，用于处理AGV的路径驱动逻辑
 *
 * 该函数是AGV路径驱动的主线程函数，负责根据当前状态和目标位置来控制AGV的移动。
 *
 * @param p 指向DriveToPath类的指针，用于访问该类的成员变量和成员函数
 */
void DriveToPath::AgvDrivingThreadFun(DriveToPath* p)
{
    if (!p) return;
    while (!p->cur_pos_is_update_) {
        if (!rclcpp::ok() || p->cur_ctrl_mode_ != static_cast<int>(CtrlMode::HalfAutoMode))
            return;
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    if (rclcpp::ok() && !p->is_on_path_ && p->cur_ctrl_mode_ == static_cast<int>(CtrlMode::HalfAutoMode)) {
        TargetPos fTargetPos = p->GetFinallyTargetPoint();
        if (fTargetPos.dis == 0 && fTargetPos.x == 0 && fTargetPos.y == 0 && fTargetPos.theta == 0) {
            RCLCPP_WARN(rclcpp::get_logger("AgvNavigate"), "[DriveToPath] Not Found TargetPos");
            return;
        }
        p->UpdateTarget(fTargetPos);
    }

    p->cur_target_is_update_ = true;
    while (!p->cur_state_is_update_) {
        if (!rclcpp::ok() || p->cur_ctrl_mode_ != static_cast<int>(CtrlMode::HalfAutoMode)) {
            return;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    p->DoDriving();
}

/**
 * @brief 主函数
 *
 * 初始化ROS节点，设置本地化，创建循环速率对象，创建DriveToPath对象，输出信息，进入ROS主循环，并在退出时输出信息。
 *
 * @param argc 命令行参数数量
 * @param argv 命令行参数列表
 * @return 返回0表示程序正常结束
 */
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto nh = rclcpp::Node::make_shared("drive_to_path");
    setlocale(LC_ALL, "");
    rclcpp::Rate loop_rate(10);
    DriveToPath driveToPath(nh);
    RCLCPP_INFO(rclcpp::get_logger("AgvNavigate"), "agv_drive_to_path set ok");
    rclcpp::spin(node);
    RCLCPP_INFO(rclcpp::get_logger("AgvNavigate"), "agv_drive_to_path Spinning node shutdown...");
    loop_rate.sleep();
    return 0;
}
